#include <stdio.h>
#include <string.h>
#include <pthread.h>
#include <stdlib.h>
#include <sys/types.h>
#include <sys/stat.h>
#include "sys_msgque.h"

#define __MALLOC_TEST_ONLY_


static t_queue_global q_data;
static unsigned long mem_alloc_count = 0;


#ifdef __MALLOC_TEST_ONLY_
void show_malloc_count(){
    printf("memory alloc count :%ld \n",mem_alloc_count);
}
#endif

void *mem_malloc(unsigned long size) {
    void *memp = malloc(size);

    if(memp == NULL){
#ifdef __SYS_DEBUG_H__
        dprint(MOD_MSGQUE,LV_ERROR,"mem_malloc(),malloc return NULL \n");
#else
        printf("sys_malloc() , malloc()failed ! S %ld \n",size);
#endif
    }

    mem_alloc_count++;

    return (memp);
}
void mem_free(void *memp) {
    if(memp == NULL){
#ifdef __SYS_DEBUG_H__
        dprint(MOD_MSGQUE,LV_ERROR,"mem_free(),free NULL \n");
#else
        printf("mem_free (): NULL memp");
#endif
    }
    else{
        free(memp);
    }
    mem_alloc_count--;
}

MQUE_RET msgque_mutex_create(pthread_mutex_t *pmutex) {
    if(pthread_mutex_init(pmutex,NULL) != 0)
    {
#ifdef __SYS_DEBUG_H__
        dprint(MOD_MSGQUE,LV_EMERG,"msgque_mutex_create() ; pthread_mutex_init() failed \n");
#else
        printf("msgque_mutex_create() ; pthread_mutex_init() failed \n");
#endif
        return (MQUE_RET_FAIL);
    }
    return (MQUE_RET_OK);
}
int msgque_mutex_lock(pthread_mutex_t *pmutex){
    if(pthread_mutex_lock(pmutex) != 0)
    {
#ifdef __SYS_DEBUG_H__
        dprint(MOD_MSGQUE,LV_EMERG,"msgque_mutex_lock() ; pthread_mutex_lock() failed \n");
#else
        printf("msgque_mutex_lock(); pthread_mutex_lock() failed \n");
#endif
        return (MQUE_RET_FAIL);
    }

    return (MQUE_RET_OK);
}


int msgque_mutex_unlock(pthread_mutex_t *pmutex){
    if(pthread_mutex_unlock(pmutex) != 0)
    {
#ifdef __SYS_DEBUG_H__
        dprint(MOD_MSGQUE,LV_EMERG,"msgque_mutex_unlock() ; pthread_mutex_unlock() failed \n");
#else
        printf("msgque_mutex_unlock(); pthread_mutex_unlock() failed !\n");
#endif
        return (MQUE_RET_FAIL);
    }

    return (MQUE_RET_OK);
}

int sys_msgque_init(void){
    if(pthread_mutex_init(&q_data.g_mutex,NULL) != 0)
    {
#ifdef __SYS_DEBUG_H__
        dprint(MOD_MSGQUE,LV_EMERG,"sys_msgque_init() ; msgque_mutex_create(q_data.g_mutex) failed  \n");
#else
        printf("sys_msgque_init(), msgque_mutex_create(q_data.g_mutex) failed !\n");
#endif
        return (MQUE_RET_FAIL);
    }

    if(pthread_mutex_init(&q_data.g_que_mutex,NULL) != 0)
    {
#ifdef __SYS_DEBUG_H__
        dprint(MOD_MSGQUE,LV_EMERG,"sys_msgque_init(), msgque_mutex_create(que_mutex) failed !\n");
#else
        printf("sys_msgque_init(), msgque_mutex_create(que_mutex) failed !\n");
#endif
        return (MQUE_RET_FAIL);
    }
    if(pthread_mutex_init(&q_data.g_sem_mutex,NULL))
    {
#ifdef __SYS_DEBUG_H__
        dprint(MOD_MSGQUE,LV_EMERG,"sys_msgque_init(), msgque_mutex_create(sem_mutex) failed !\n");
#else
        printf("sys_msgque_init(), msgque_mutex_create(sem_mutex) failed !\n");
#endif
        return (MQUE_RET_FAIL);
    }

    q_data.que_num = 0;
    q_data.semnum = 0;

    if((netrecv_id = sys_msgque_create("net_recv")) == MQUE_RET_FAIL)
    {
#ifdef __SYS_DEBUG_H__
        dprint(MOD_MSGQUE,LV_EMERG,"sys_msgque_init(); msg_net_recv init failed!\n");
#else
        printf("sys_msgque_init(); msg_net_recv init failed!\n");
#endif
        return MQUE_RET_FAIL;
    }   

    if((netsend_id = sys_msgque_create("net_send")) == MQUE_RET_FAIL)
    {
#ifdef __SYS_DEBUG_H__
        dprint(MOD_MSGQUE,LV_EMERG,"sys_msgque_init(); msg_net_send init failed!\n");
#else
        printf("sys_msgque_init(); msg_net_send init failed!\n");
#endif
        return MQUE_RET_FAIL;
    }   

    if((ppcrecv_id = sys_msgque_create("uart_recv")) == MQUE_RET_FAIL)
    {
#ifdef __SYS_DEBUG_H__
        dprint(MOD_MSGQUE,LV_EMERG,"sys_msgque_init(); msg_ppc_recv init failed!\n");
#else
        printf("sys_msgque_init(); msg_ppc_recv init failed!\n");
#endif
        return MQUE_RET_FAIL;
    }   

    if((ppcsend_id = sys_msgque_create("uart_recv")) == MQUE_RET_FAIL)
    {
#ifdef __SYS_DEBUG_H__
        dprint(MOD_MSGQUE,LV_EMERG,"sys_msgque_init(); msg_ppc_send init failed!\n");
#else
        printf("sys_msgque_init(); msg_ppc_send init failed!\n");
#endif
        return MQUE_RET_FAIL;
    }   
    return MQUE_RET_OK;
}

int msgque_sem_create(char *pname,t_semid *psemid)
{
    t_scb *pscb = NULL;
    int semindex;
    *psemid = (t_semid)NULL;

    if(pname == NULL)
    {
        printf("msgque_sem_create() pname == NULL !\n");
        return (MQUE_RET_ARG_ERROR);
    }
    msgque_mutex_lock(&q_data.g_sem_mutex);
    semindex = q_data.semnum;  
    if(semindex >= OS_WRAP_MAX_SEMS)
    {
        msgque_mutex_unlock(&q_data.g_sem_mutex);
#ifdef __SYS_DEBUG_H__
        dprint(MOD_MSGQUE,LV_EMERG,"msgque_sem_create(), semindex > max sems limit \n");
#else
        printf("msgque_sem_create(), semindex > max sems limit \n");
#endif
        return (MQUE_RET_FAIL);
    }
    pscb = &(q_data.scb_array[semindex]);  
    if(msgque_mutex_create(&pscb->sem_mutex) != MQUE_RET_OK)
    {
        msgque_mutex_unlock(&q_data.g_sem_mutex);
#ifdef __SYS_DEBUG_H__
        dprint(MOD_MSGQUE,LV_EMERG,"msgque_sem_create(),msgque_mutex_create(&pscb->sem_mutex) failed \n");
#else
        printf("msgque_sem_create(),msgque_mutex_create(&pscb->sem_mutex) failed\n");
#endif
        return (MQUE_RET_FAIL);
    }
    if(pthread_cond_init(&pscb->cond_var,NULL) != 0)    
    {
        pthread_mutex_destroy(&pscb->sem_mutex);
        msgque_mutex_unlock(&q_data.g_sem_mutex);
#ifdef __SYS_DEBUG_H__
        dprint(MOD_MSGQUE,LV_EMERG,"msgque_sem_create(),pthread_cond_init(&pscb->cond_var,NULL) failed\n");
#else
        printf("msgque_sem_create(),pthread_cond_init(&pscb->cond_var,NULL) failed\n");
#endif
        return (MQUE_RET_FAIL);
    }
    pscb->semid = semindex + 1; 
    pscb->sem_count = 0;
    q_data.semnum++;
    *psemid = pscb->semid;
    memcpy(pscb->name,pname,sizeof(pscb->name) - 1);
    msgque_mutex_unlock(&q_data.g_sem_mutex);
    return (MQUE_RET_OK);
}

int msgque_sem_release(t_semid semid)
{
    t_scb *pscb;
    int semindex;

    for(semindex=0;semindex < q_data.semnum;semindex++)
    {
        if(q_data.scb_array[semindex].semid == semid)
        {
            break;
        }
    }
    if(semindex >= q_data.semnum)
    {
#ifdef __SYS_DEBUG_H__
        dprint(MOD_MSGQUE,LV_EMERG,"msgque_sem_release(); semindex > q_data.semnum ,No such semid ! \n");
#else
        printf("msgque_sem_release(); semindex > q_data.semnum ,No such semid ! \n");
#endif
        return (MQUE_RET_NO_EXIST);
    }
    pscb = &q_data.scb_array[semindex];
    msgque_mutex_lock(&pscb->sem_mutex);
    pscb->sem_count++;  
    if(pscb->sem_count == 1){
        pthread_cond_signal(&pscb->cond_var);  
    }
    msgque_mutex_unlock(&pscb->sem_mutex);
    return (MQUE_RET_OK);
}

int msgque_sem_wait(t_semid semid)
{
    t_scb *pscb;
    int semindex;

    for(semindex = 0;semindex < q_data.semnum;semindex++)
    {
        if(q_data.scb_array[semindex].semid == semid)
        {
            break;
        }
    }
    if(semindex >= q_data.semnum)
    {
#ifdef __SYS_DEBUG_H__
        dprint(MOD_MSGQUE,LV_EMERG,"msgque_sem_wait(); semindex > q_data.semnum ,No such semid ! \n");
#else
        printf("msgque_sem_wait(); semindex > q_data.semnum ,No such semid ! \n");
#endif
        return (MQUE_RET_NO_EXIST);
    }

    pscb = &q_data.scb_array[semindex];

    msgque_mutex_lock(&pscb->sem_mutex);

    if(pscb->sem_count == 0)  
    {
        if(pthread_cond_wait(&pscb->cond_var,&pscb->sem_mutex) != 0)
        {
            return MQUE_RET_FAIL;
        }
    }
    if(pscb->sem_count > 0)
    {
        pscb->sem_count--;  
    }
    msgque_mutex_unlock(&pscb->sem_mutex);
    return (MQUE_RET_OK);
}

unsigned long sys_msgque_create(char *p_name)
{
    t_qcb *pqcb = NULL;
    int qindex;
    t_queid queid;

    if(p_name == NULL)
    {
#ifdef __SYS_DEBUG_H__
        dprint(MOD_MSGQUE,LV_EMERG,"sys_msgque_create();p_name == NULL \n");
#else
        printf("sys_msgque_create();p_name == NULL \n");
#endif
        return (MQUE_RET_FAIL);
    }
    msgque_mutex_lock(&q_data.g_que_mutex);
    qindex = q_data.que_num; 
    if(qindex >= OS_WRAP_MAX_QUEUES)
    {
        msgque_mutex_unlock(&q_data.g_que_mutex);
#ifdef __SYS_DEBUG_H__
        dprint(MOD_MSGQUE,LV_EMERG,"sys_msgque_create();p_name == NULL \n");
#else
        printf("sys_msgque_create(); qindex >= OS_WARP_MAX_QUEUES \n");
#endif
        return (MQUE_RET_FAIL);
    }

    pqcb = &(q_data.qcb_array[qindex]);

    if(msgque_sem_create(p_name,&pqcb->semid) != MQUE_RET_OK)
    {
#ifdef __SYS_DEBUG_H__
        dprint(MOD_MSGQUE,LV_EMERG,"sys_msgque_create(); msgque_sem_create() != 0 \n");
#else
        printf("sys_msgque_create(); msgque_sem_create() != 0 \n");
#endif
        msgque_mutex_unlock(&q_data.g_que_mutex);
        return (MQUE_RET_FAIL);
    }

    if(msgque_mutex_create(&pqcb->qmutex) != MQUE_RET_OK)
    {
#ifdef __SYS_DEBUG_H__
        dprint(MOD_MSGQUE,LV_EMERG,"sys_msgque_create(); msgque_mutex_create() != 0 \n");
#else
        printf("sys_msgque_create(); msgque_mutex_create() != 0 \n");
#endif
        msgque_mutex_unlock(&q_data.g_que_mutex);
        return (MQUE_RET_FAIL);
    }
    pqcb->firstp = pqcb->lastp;    
    q_data.que_num++;
    queid = qindex + 1;    
    memcpy(pqcb->name,p_name,sizeof(pqcb->name)-1);   
    pqcb->name[sizeof(pqcb->name)-1] = '\x0';
    msgque_mutex_unlock(&q_data.g_que_mutex);

    return queid;

}

unsigned short sys_msgque_send(unsigned long qid,void *msg,unsigned short msglen)
{
    t_msg_block *pbmsg;
    t_qcb *pqcb;
    unsigned long qindex;

    if(msg == NULL)
    {
#ifdef __SYS_DEBUG_H__
        dprint(MOD_MSGQUE,LV_EMERG,"sys_msgque_send(),msg == NULL \n");
#else
        printf("sys_msgque_send(),msg == NULL \n");
#endif
        return (MQUE_RET_ARG_ERROR);
    }

    qindex = qid - 1;

    if(qindex >= OS_WRAP_MAX_QUEUES)
    {
#ifdef __SYS_DEBUG_H__
        dprint(MOD_MSGQUE,LV_EMERG,"sys_msgque_send();qid error\n");
#else
        printf("sys_msgque_send();qid error\n");
#endif
        return (MQUE_RET_ARG_ERROR);
    }

    pqcb = &(q_data.qcb_array[qindex]);

    pbmsg = (t_msg_block *)mem_malloc(sizeof(t_msg_block));   

    if(pbmsg == NULL)
    {
#ifdef __SYS_DEBUG_H__
        dprint(MOD_MSGQUE,LV_EMERG,"sys_msgque_send(),mem_malloc pbmsg == NULL \n");
#else
        printf("sys_msgque_send(),mem_malloc pbmsg == NULL \n");
#endif
        return (MQUE_RET_FAIL);
    }

    pbmsg->nextp = NULL;    
    pbmsg->msg = msg;   
    pbmsg->msglen = msglen;  
	//printf("%s\n", pbmsg ->msg);

msgque_mutex_lock(&pqcb->qmutex);

    if(pqcb->lastp != NULL)
    {
        pqcb->lastp->nextp = pbmsg;
        pqcb->lastp = pbmsg;
    }
    else
    {
        pqcb->firstp = pqcb->lastp = pbmsg;
    }

    msgque_sem_release(pqcb->semid); 

    msgque_mutex_unlock(&pqcb->qmutex);

    return (MQUE_RET_SUCCESS);

}
unsigned short sys_msgque_recv(unsigned long qid,void **msg,unsigned short *msglen)
{
    t_msg_block *pbmsg;
    t_qcb *pqcb;
    unsigned long qindex;

    pbmsg = NULL;
    qindex = qid - 1;

    if(qindex >= OS_WRAP_MAX_QUEUES)
    {
#ifdef __SYS_DEBUG_H__
        dprint(MOD_MSGQUE,LV_EMERG,"sys_msgque_recv(); qindex >= limit \n");
#else
        printf("sys_msgque_recv(); qindex >= limit \n");
#endif
        return (MQUE_RET_ARG_ERROR);
    }

    pqcb = &q_data.qcb_array[qindex];

    if(msgque_sem_wait(pqcb->semid) != MQUE_RET_OK)
    {
#ifdef __SYS_DEBUG_H__
        dprint(MOD_MSGQUE,LV_EMERG,"sys_msgque_recv(); msgque_sem_wait () failed \n");
#else
        printf("sys_msgque_recv(); msgque_sem_wait () failed \n");
#endif
        return (MQUE_RET_FAIL);
    }

    msgque_mutex_lock(&pqcb->qmutex);

    pbmsg = pqcb->firstp;   

    if(pbmsg == NULL)
    {
#ifdef __SYS_DEBUG_H__
        dprint(MOD_MSGQUE,LV_EMERG,"sys_msgque_recv(); pbmsg == NULL \n");
#else
        printf("sys_msgque_recv(); pbmsg == NULL \n");
#endif
        msgque_mutex_unlock(&pqcb->qmutex);
        return (MQUE_RET_FAIL);
    }

    *msg = pbmsg->msg;   
    *msglen = pbmsg->msglen;   

    if(pqcb->lastp == pbmsg) 
    {
        pqcb->lastp = NULL;
    }
    pqcb->firstp = pbmsg->nextp;

    mem_free(pbmsg); 
    msgque_mutex_unlock(&pqcb->qmutex);

    return (MQUE_RET_SUCCESS);

}


int main(void)
{
	MQUE_RET ret; 
	char *msg= "Hello the crule world\n";
	char *send_msg;
	void *recv_buff = NULL;
	unsigned short int msglen = 0;
	int i = 0;
	
	send_msg = (char *)malloc(strlen(msg) + 1);
	memcpy(send_msg, msg, strlen(msg));
	*(send_msg + strlen(msg)) = '\0';
	
	ret = sys_msgque_init();
	if(MQUE_RET_OK != ret){
		printf("msgque init error\n");
		goto end;
	}
	
	printf("net_recv: %ld  net_send: %ld  ppc_recv: %ld ppc_send: %ld\n", netrecv_id, netsend_id, ppcrecv_id, ppcsend_id);

	sys_msgque_send(netsend_id, (void*)send_msg, strlen(send_msg));
	
	sys_msgque_recv(netsend_id, (void **)&recv_buff,&msglen);
	for(i = 0; i < msglen; i++)
		printf("%c ", *((char *)(recv_buff + i)));
	printf("\n");
	free(recv_buff);
end:
	return 0;
}
